#include "mult_lidar/mult_lidar.hpp"
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>

namespace mult_lidar{

    MultLidarNode::MultLidarNode() : Node("mult_lidar_node") {
        
        //声明参数
        declare_parameters();
        declare_tf_parameters();

        //初始化处理组件
        init_components();

        //创建订阅者和发布者
        setup_ros_interface();

        RCLCPP_INFO(this->get_logger(), "multlidar node is now working!!!");
    }

    void MultLidarNode::declare_parameters(){

        //点云处理参数
        this->declare_parameter("radar_height", 1.10);
        this->declare_parameter("rotation_angle", 3.80);
        this->declare_parameter("merged.max_frames", 6);

        // 这里保留其他参数，但移除detection_manager相关参数
        this->declare_parameter("intensity_threshold", 38.0);
        this->declare_parameter("voxel_leaf_size", 0.02);
        this->declare_parameter("x_min", 0.0);
        this->declare_parameter("x_max", 1.9);
        this->declare_parameter("y_min", -2.5);
        this->declare_parameter("y_max", 2.5);
        this->declare_parameter("z_min", 0.02);
        this->declare_parameter("z_max", 1.2);
        this->declare_parameter("sor_mean_k", 500);
        this->declare_parameter("sor_std_mul", 1.2);

        //放射源桶中心检测参数
        this->declare_parameter("barrel.ring_height_threshold", 0.2);
        this->declare_parameter("barrel.barrel_radius", 0.2);
        this->declare_parameter("barrel.radius_filter_margin", 0.10);
        this->declare_parameter("barrel.search_start_z", 0.6);
        this->declare_parameter("barrel.search_end_z", 0.4);
        this->declare_parameter("barrel.search_step", 0.01);
        this->declare_parameter("barrel.min_points_in_slice", 30);
        this->declare_parameter("barrel.min_points_after_filter", 10);
        this->declare_parameter("barrel.max_radius_error", 0.1);

        //井盖中心检测参数
        this->declare_parameter("manhole.handle_search_start_z", 0.6);
        this->declare_parameter("manhole.handle_search_end_z", 0.2);
        this->declare_parameter("manhole.handle_search_step", 0.01);
        this->declare_parameter("manhole.handle_height_range", 0.13);
        this->declare_parameter("manhole.handle_min_points", 200);
        this->declare_parameter("manhole.handle_z_offset", 0.13);

        this->declare_parameter("manhole.circle_search_start_z", 0.33);
        this->declare_parameter("manhole.circle_search_end_z", 0.2);
        this->declare_parameter("manhole.circle_search_step", 0.01);
        this->declare_parameter("manhole.circle_height_range", 0.1);
        this->declare_parameter("manhole.circle_min_points", 50);
        this->declare_parameter("manhole.circle_min_points_final", 30);

        this->declare_parameter("manhole.quantization_step", 0.01);
        this->declare_parameter("manhole.percentile_low", 0.05);
        this->declare_parameter("manhole.percentile_high", 0.95);
        this->declare_parameter("manhole.center_merge_threshold", 0.03);

        //锥形放射源检测参数
        this->declare_parameter("neutron.search_start_z", 0.35);
        this->declare_parameter("neutron.search_end_z", 0.2);
        this->declare_parameter("neutron.search_step", 0.01);
        this->declare_parameter("neutron.height_range", 0.2);
        this->declare_parameter("neutron.height_offset", 0.1);
        this->declare_parameter("neutron.min_points_in_slice", 50);

        this->declare_parameter("neutron.rect_min_area", 0.01);
        this->declare_parameter("neutron.rect_max_area", 0.1);
        this->declare_parameter("neutron.circle_min_radius", 0.02);
        this->declare_parameter("neutron.circle_max_radius", 0.15);
        this->declare_parameter("neutron.circle_min_points", 20);
        this->declare_parameter("neutron.center_merge_threshold", 0.03);

        //检测器模式选择参数
        this->declare_parameter("detection_mode", "both");
    }

    void MultLidarNode::declare_tf_parameters() {
        
        this->declare_parameter<std::string>("lidar_frame", "lidar_link");
        this->declare_parameter<std::string>("base_frame", "base_link");
        this->declare_parameter<bool>("publish_tf", true);
        
        
        this->get_parameter("lidar_frame", lidar_frame_);
        this->get_parameter("base_frame", base_frame_);
        this->get_parameter("publish_tf", publish_tf_);
    }

    void MultLidarNode::publish_static_tf() {
        //初始化tf广播器
        static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(
            std::shared_ptr<rclcpp::Node>(this, [](auto*){}));
        
        tf_initialized_ = true;

        // 创建变换
        geometry_msgs::msg::TransformStamped static_transform;
        static_transform.header.stamp = this->now();
        static_transform.header.frame_id = base_frame_;
        static_transform.child_frame_id = lidar_frame_;
        
        // 设置激光雷达相对于 base_link 的位置和姿态
        static_transform.transform.translation.x = 0.885;
        static_transform.transform.translation.y = 0.0;
        static_transform.transform.translation.z = 0.565;
        
        static_transform.transform.rotation.x = 0.0;
        static_transform.transform.rotation.y = 0.0;
        static_transform.transform.rotation.z = 0.0;
        static_transform.transform.rotation.w = 1.0;
        
        // 发布静态变换
        static_tf_broadcaster_->sendTransform(static_transform);
        
        RCLCPP_INFO(this->get_logger(), "Published static TF from %s to %s", 
                    base_frame_.c_str(), lidar_frame_.c_str());
    }

    void MultLidarNode::init_components(){

        //初始化点云处理器
        processor_ = std::make_unique<PointCloudProcessor>();

        ProcessingParams params;
        this->get_parameter("radar_height", params.radar_height);
        this->get_parameter("rotation_angle", params.rotation_angle);
        this->get_parameter("max_frames", params.max_frames);
        this->get_parameter("intensity_threshold", params.intensity_threshold);
        this->get_parameter("voxel_leaf_size", params.voxel_leaf_size);
        this->get_parameter("x_min", params.x_min);
        this->get_parameter("x_max", params.x_max);
        this->get_parameter("y_min", params.y_min);
        this->get_parameter("y_max", params.y_max);
        this->get_parameter("z_min", params.z_min);
        this->get_parameter("z_max", params.z_max);
        this->get_parameter("sor_mean_k", params.sor_mean_k);
        this->get_parameter("sor_std_mul", params.sor_std_mul);

        this->get_parameter("detection_mode", detection_mode_);

        processor_->setParameters(params);

        //初始化监测管理器 - 从类内参数获取，而不是从yaml
        detection_manager_ = std::make_unique<DetectionManager>();

        //创建桶检测器
        auto barrel_detector = std::make_shared<BarrelDetector>();
        detection_manager_->registerDetector("barrel", barrel_detector);

        //创建井盖检测器
        auto manhole_detector = std::make_shared<ManholeDetector>();
        detection_manager_->registerDetector("manhole", manhole_detector);

        //创建锥形放射源检测器
        auto neutron_detector = std::make_shared<NeutronDetector>();
        detection_manager_->registerDetector("neutron", neutron_detector);

        this->get_parameter("merged.max_frames", merged_max_frames_);
        this->get_parameter("merged.enable_publish", enable_merged_cloud_publish_);
        this->get_parameter("merged.publish_frequency", publish_frequency_);

        //初始化合并点云
        merged_cloud_ = std::make_unique<Merged_cloud>(merged_max_frames_);

        //设置发布者
        barrel_detector->setRingCloudPublisher(pub_ring_cloud_);

        // 初始化坐标发布者
        init_coordinate_publishers();
    }

    void MultLidarNode::init_coordinate_publishers() {
        // 为每种放射源类型创建坐标发布者
        pub_barrel_coordinates_ = this->create_publisher<geometry_msgs::msg::PointStamped>("/barrel_coordinates", 10);
        pub_manhole_coordinates_ = this->create_publisher<geometry_msgs::msg::PointStamped>("/manhole_coordinates", 10);
        pub_neutron_coordinates_ = this->create_publisher<geometry_msgs::msg::PointStamped>("/neutron_coordinates", 10);
    }

    void MultLidarNode::setup_ros_interface(){
        
        //订阅点云消息
        cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "transformed_cloud", 10,
            std::bind(&MultLidarNode::cloud_callback, this, std::placeholders::_1));

        //创建发布者
        pub_accumulated_cloud_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/accumulated_cloud", 10);
        pub_center_ = this->create_publisher<geometry_msgs::msg::PointStamped>("/barrel_center", 10);
        pub_handle_center_ = this->create_publisher<geometry_msgs::msg::PointStamped>("/handle_center", 10);
        pub_neutron_center_ = this->create_publisher<geometry_msgs::msg::PointStamped>("/neutron_center", 10);

        //设置检测器参数
        auto barrel_detector = std::dynamic_pointer_cast<BarrelDetector>(
            detection_manager_->getDetector("barrel"));
        if (barrel_detector) {
            barrel_detector->setParameters(this);
        }
        
        auto manhole_detector = std::dynamic_pointer_cast<ManholeDetector>(
            detection_manager_->getDetector("manhole"));
        if (manhole_detector) {
            manhole_detector->setParameters(this);
        }

        auto neutron_detector = std::dynamic_pointer_cast<NeutronDetector>(
            detection_manager_->getDetector("neutron"));
        if (neutron_detector) {
            neutron_detector->setParameters(this);
        }
    }

    void MultLidarNode::cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {

        //转换为PCL点云
        pcl::PointCloud<pcl::PointXYZI>::Ptr laser_cloud(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::fromROSMsg(*msg, *laser_cloud);

        if(laser_cloud->empty()) {
            return;
        }

        //初始化并发布tf
        if(!tf_initialized_ && publish_tf_) {
            publish_static_tf();
            tf_initialized_ = true;
        }

        //点云帧数累积 - 移除预处理步骤
        merged_cloud_->add_cloud(laser_cloud);

        int current_frame_count = merged_cloud_->get_fram_sum();
            
        RCLCPP_INFO(this->get_logger(), "merged frames from %d to %d, points: %ld", 
                current_frame_count, 
                merged_max_frames_,
                merged_cloud_->get_merged_cloud()->size()); 

        // 当累计足够多帧数时进行目标检测
        if(current_frame_count >= merged_max_frames_) {
            perform_detection();
        }
    }

    // 移除 process_pointcloud 函数，因为不再需要预处理

    void MultLidarNode::publish_pointcloud(const std_msgs::msg::Header& header) {
        auto merged_cloud = merged_cloud_->get_merged_cloud();
        sensor_msgs::msg::PointCloud2 output_msg;
        pcl::toROSMsg(*merged_cloud, output_msg);
        output_msg.header = header;
        output_msg.header.stamp = this->now();
        output_msg.header.frame_id = "map";
        pub_accumulated_cloud_->publish(output_msg);
    }
    
    void MultLidarNode::perform_detection() {
        auto merged_cloud = merged_cloud_->get_merged_cloud();

        //仅使用桶检测
        if(detection_mode_ == "barrel_only") {
            geometry_msgs::msg::PointStamped barrel_center;
            if(detection_manager_->detect("barrel", merged_cloud, barrel_center)) {
                barrel_center.header.stamp = this->now();
                pub_center_->publish(barrel_center);
                
                // 发布桶的坐标
                geometry_msgs::msg::PointStamped barrel_coord;
                barrel_coord.header = barrel_center.header;
                barrel_coord.point = barrel_center.point;
                pub_barrel_coordinates_->publish(barrel_coord);
            }
        }

        //仅使用井盖检测
        else if(detection_mode_ == "manhole_only") {
            geometry_msgs::msg::PointStamped handle_center;
            if(detection_manager_->detect("manhole", merged_cloud, handle_center)) {
                handle_center.header.stamp = this->now();
                pub_handle_center_->publish(handle_center);
                
                // 发布井盖的坐标
                geometry_msgs::msg::PointStamped manhole_coord;
                manhole_coord.header = handle_center.header;
                manhole_coord.point = handle_center.point;
                pub_manhole_coordinates_->publish(manhole_coord);
            }
        }

        //仅使用锥形放射源检测
        else if(detection_mode_ == "neutron_only") {
            geometry_msgs::msg::PointStamped neutron_center;
            if(detection_manager_->detect("neutron", merged_cloud, neutron_center)) {
                neutron_center.header.stamp = this->now();
                pub_neutron_center_->publish(neutron_center);
                
                // 发布中子源的坐标
                geometry_msgs::msg::PointStamped neutron_coord;
                neutron_coord.header = neutron_center.header;
                neutron_coord.point = neutron_center.point;
                pub_neutron_coordinates_->publish(neutron_coord);
            }
        }

        // 同时使用多种检测器
        else if(detection_mode_ == "both" || detection_mode_ == "all") {
            //桶检测
            geometry_msgs::msg::PointStamped barrel_center;
            if(detection_manager_->detect("barrel", merged_cloud, barrel_center)) {
                barrel_center.header.stamp = this->now();
                pub_center_->publish(barrel_center);
                
                // 发布桶的坐标
                geometry_msgs::msg::PointStamped barrel_coord;
                barrel_coord.header = barrel_center.header;
                barrel_coord.point = barrel_center.point;
                pub_barrel_coordinates_->publish(barrel_coord);
            }

            //井盖检测
            geometry_msgs::msg::PointStamped handle_center;
            if(detection_manager_->detect("manhole", merged_cloud, handle_center)) {
                handle_center.header.stamp = this->now();
                pub_handle_center_->publish(handle_center);
                
                // 发布井盖的坐标
                geometry_msgs::msg::PointStamped manhole_coord;
                manhole_coord.header = handle_center.header;
                manhole_coord.point = handle_center.point;
                pub_manhole_coordinates_->publish(manhole_coord);
            }

            //锥形放射源检测
            geometry_msgs::msg::PointStamped neutron_center;
            if(detection_manager_->detect("neutron", merged_cloud, neutron_center)) {
                neutron_center.header.stamp = this->now();
                pub_neutron_center_->publish(neutron_center);
                
                // 发布中子源的坐标
                geometry_msgs::msg::PointStamped neutron_coord;
                neutron_coord.header = neutron_center.header;
                neutron_coord.point = neutron_center.point;
                pub_neutron_coordinates_->publish(neutron_coord);
            }
        }
    }
}

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<mult_lidar::MultLidarNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0 ;
}